package com.smile.commons.gps;

import com.smile.commons.gps.domain.CoordinatePointReq;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.ArrayList;
import java.util.List;

/**
 * 航线路径优化工具类
 * 提供航线路径优化和调整功能
 *
 * @author System
 * @since 2024-09-18
 */
public class AirlinePathOptimizer {

    /**
     * 优化航线路径 - 去除冗余点和平滑路径
     *
     * @param flightPath 原始航线路径
     * @param tolerance 简化容差（度）
     * @return 优化后的航线路径
     */
    public static List<CoordinatePointReq> optimizeFlightPath(List<CoordinatePointReq> flightPath, double tolerance) {
        if (flightPath == null || flightPath.size() <= 2) {
            return flightPath;
        }

        List<CoordinatePointReq> optimizedPath = new ArrayList<>();
        optimizedPath.add(flightPath.get(0)); // 添加起始点

        // 使用道格拉斯-普克算法简化路径
        douglasPeucker(flightPath, 0, flightPath.size() - 1, tolerance, optimizedPath);

        optimizedPath.add(flightPath.get(flightPath.size() - 1)); // 添加结束点

        return optimizedPath;
    }

    /**
     * 道格拉斯-普克算法实现
     */
    private static void douglasPeucker(List<CoordinatePointReq> points, int start, int end, double tolerance, List<CoordinatePointReq> result) {
        if (end - start <= 1) {
            return;
        }

        double maxDistance = 0;
        int maxIndex = 0;

        // 找到距离起终点连线最远的点
        for (int i = start + 1; i < end; i++) {
            double distance = pointToLineDistance(
                    points.get(i),
                    points.get(start),
                    points.get(end)
            );
            if (distance > maxDistance) {
                maxDistance = distance;
                maxIndex = i;
            }
        }

        // 如果最大距离大于容差，递归处理
        if (maxDistance > tolerance) {
            douglasPeucker(points, start, maxIndex, tolerance, result);
            result.add(points.get(maxIndex));
            douglasPeucker(points, maxIndex, end, tolerance, result);
        }
    }

    /**
     * 计算点到直线的距离
     */
    private static double pointToLineDistance(CoordinatePointReq point, CoordinatePointReq lineStart, CoordinatePointReq lineEnd) {
        double x0 = point.getLongitude().doubleValue();
        double y0 = point.getLatitude().doubleValue();
        double x1 = lineStart.getLongitude().doubleValue();
        double y1 = lineStart.getLatitude().doubleValue();
        double x2 = lineEnd.getLongitude().doubleValue();
        double y2 = lineEnd.getLatitude().doubleValue();

        double numerator = Math.abs((y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1);
        double denominator = Math.sqrt((y2 - y1) * (y2 - y1) + (x2 - x1) * (x2 - x1));

        return denominator == 0 ? 0 : numerator / denominator;
    }

    /**
     * 添加转弯点 - 在航线转弯处添加中间点以平滑转弯
     *
     * @param flightPath 航线路径
     * @param turnRadius 转弯半径（米）
     * @return 添加转弯点后的路径
     */
    public static List<CoordinatePointReq> addTurnPoints(List<CoordinatePointReq> flightPath, double turnRadius) {
        if (flightPath == null || flightPath.size() <= 2) {
            return flightPath;
        }

        List<CoordinatePointReq> smoothPath = new ArrayList<>();
        smoothPath.add(flightPath.get(0)); // 添加起始点

        for (int i = 1; i < flightPath.size() - 1; i++) {
            CoordinatePointReq prev = flightPath.get(i - 1);
            CoordinatePointReq current = flightPath.get(i);
            CoordinatePointReq next = flightPath.get(i + 1);

            // 计算转弯角度
            double angle = calculateTurnAngle(prev, current, next);

            // 如果转弯角度大于阈值，添加转弯点
            // 增加角度阈值到60度，减少不必要的转弯点生成
            if (Math.abs(angle) > Math.toRadians(60)) { // 从30度增加到60度阈值
                // 只添加原始转弯点，不再额外添加前后过渡点
                smoothPath.add(current);
            } else {
                smoothPath.add(current);
            }
        }

        smoothPath.add(flightPath.get(flightPath.size() - 1)); // 添加结束点
        return smoothPath;
    }

    /**
     * 计算转弯角度
     */
    private static double calculateTurnAngle(CoordinatePointReq p1, CoordinatePointReq p2, CoordinatePointReq p3) {
        double lat1 = Math.toRadians(p1.getLatitude().doubleValue());
        double lon1 = Math.toRadians(p1.getLongitude().doubleValue());
        double lat2 = Math.toRadians(p2.getLatitude().doubleValue());
        double lon2 = Math.toRadians(p2.getLongitude().doubleValue());
        double lat3 = Math.toRadians(p3.getLatitude().doubleValue());
        double lon3 = Math.toRadians(p3.getLongitude().doubleValue());

        // 计算方位角
        double bearing1 = Math.atan2(Math.sin(lon2 - lon1) * Math.cos(lat2),
                Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(lon2 - lon1));
        double bearing2 = Math.atan2(Math.sin(lon3 - lon2) * Math.cos(lat3),
                Math.cos(lat2) * Math.sin(lat3) - Math.sin(lat2) * Math.cos(lat3) * Math.cos(lon3 - lon2));

        double angle = bearing2 - bearing1;

        // 标准化角度到[-π, π]
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;

        return angle;
    }

    /**
     * 生成转弯点
     */
    private static List<CoordinatePointReq> generateTurnPoints(CoordinatePointReq p1, CoordinatePointReq p2, CoordinatePointReq p3, double turnRadius) {
        List<CoordinatePointReq> turnPoints = new ArrayList<>();

        // 简化版本：在转弯点前后添加中间点
        CoordinatePointReq beforeTurn = interpolatePoint(p1, p2, 0.8);
        CoordinatePointReq afterTurn = interpolatePoint(p2, p3, 0.2);

        turnPoints.add(beforeTurn);
        turnPoints.add(p2);
        turnPoints.add(afterTurn);

        return turnPoints;
    }

    /**
     * 在两点间插值
     */
    private static CoordinatePointReq interpolatePoint(CoordinatePointReq p1, CoordinatePointReq p2, double ratio) {
        double lat1 = p1.getLatitude().doubleValue();
        double lon1 = p1.getLongitude().doubleValue();
        double lat2 = p2.getLatitude().doubleValue();
        double lon2 = p2.getLongitude().doubleValue();

        double lat = lat1 + (lat2 - lat1) * ratio;
        double lon = lon1 + (lon2 - lon1) * ratio;

        CoordinatePointReq result = new CoordinatePointReq();
        result.setLatitude(new BigDecimal(lat).setScale(8, RoundingMode.HALF_UP));
        result.setLongitude(new BigDecimal(lon).setScale(8, RoundingMode.HALF_UP));
        result.setHeight(p1.getHeight()); // 保持相同高度

        return result;
    }

    /**
     * 计算航线总长度
     *
     * @param flightPath 航线路径
     * @return 总长度（米）
     */
    public static double calculateTotalDistance(List<CoordinatePointReq> flightPath) {
        if (flightPath == null || flightPath.size() < 2) {
            return 0;
        }

        double totalDistance = 0;
        for (int i = 1; i < flightPath.size(); i++) {
            totalDistance += calculateDistance(flightPath.get(i - 1), flightPath.get(i));
        }

        return totalDistance;
    }

    /**
     * 计算两点间距离（米）
     */
    private static double calculateDistance(CoordinatePointReq p1, CoordinatePointReq p2) {
        double lat1 = Math.toRadians(p1.getLatitude().doubleValue());
        double lon1 = Math.toRadians(p1.getLongitude().doubleValue());
        double lat2 = Math.toRadians(p2.getLatitude().doubleValue());
        double lon2 = Math.toRadians(p2.getLongitude().doubleValue());

        double deltaLat = lat2 - lat1;
        double deltaLon = lon2 - lon1;

        double a = Math.sin(deltaLat / 2) * Math.sin(deltaLat / 2) +
                Math.cos(lat1) * Math.cos(lat2) *
                        Math.sin(deltaLon / 2) * Math.sin(deltaLon / 2);
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));

        return 6378137.0 * c; // 地球半径 * 弧度
    }

    /**
     * 估算飞行时间
     *
     * @param flightPath 航线路径
     * @param speed 飞行速度（米/秒）
     * @return 预估飞行时间（秒）
     */
    public static double estimateFlightTime(List<CoordinatePointReq> flightPath, double speed) {
        double distance = calculateTotalDistance(flightPath);
        return distance / speed;
    }

    /**
     * 估算照片数量
     *
     * @param flightPath 航线路径
     * @param photoInterval 拍照间隔（米）
     * @return 预估照片数量
     */
    public static int estimatePhotoCount(List<CoordinatePointReq> flightPath, double photoInterval) {
        double distance = calculateTotalDistance(flightPath);
        return (int) Math.ceil(distance / photoInterval);
    }
}